//
//  astarcpp.cpp
//  intmap
//
//  Created by liwei on 13-11-10.
//  Copyright (c) 2013年 liwei. All rights reserved.
//

#include "astarcpp.h"
#include <stdio.h>

//
// 地图碰撞数据
//
// 1  可通行
// 2  不可通行
//

const char p_      =    '.';
const char s_      =    '&';
const char g_idle_point   =    p_;

//char map[ MAP_HEIGHT ][ MAP_WIDTH + 1] =
//{
//    p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_,'\0',
//    p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_,'\0',
//    p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_,'\0',
//    p_, p_, p_, p_, p_, p_, p_, p_, p_, s_, p_, p_, p_, p_, p_, p_,'\0',
//    p_, p_, p_, p_, p_, p_, p_, p_, p_, s_, p_, p_, p_, p_, p_, p_,'\0',
//    p_, p_, p_, p_, p_, p_, p_, p_, p_, s_, p_, p_, p_, p_, p_, p_,'\0',
//    p_, p_, p_, p_, p_, p_, p_, p_, p_, s_, p_, p_, p_, p_, p_, p_,'\0',
//    p_, p_, p_, p_, p_, p_, p_, p_, p_, s_, p_, p_, p_, p_, p_, p_,'\0',
//    p_, p_, p_, p_, p_, p_, p_, p_, p_, s_, p_, p_, p_, p_, p_, p_,'\0',
//    p_, p_, p_, p_, p_, p_, p_, p_, p_, s_, p_, p_, p_, p_, p_, p_,'\0',
//    p_, p_, p_, p_, p_, p_, p_, p_, p_, s_, p_, p_, p_, p_, p_, p_,'\0',
//    p_, p_, p_, p_, p_, p_, p_, p_, p_, s_, p_, p_, p_, p_, p_, p_,'\0',
//    p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_,'\0',
//    p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_,'\0',
//    p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_,'\0',
//    p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_,'\0'
//};

char map[ MAP_HEIGHT ][ MAP_WIDTH + 1] =
{
    p_, p_, p_, p_, p_, p_, p_, p_, p_, s_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_,'\0',
    p_, p_, p_, p_, p_, p_, p_, p_, p_, s_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_,'\0',
    p_, p_, p_, p_, p_, p_, p_, p_, p_, s_, p_, p_, p_, p_, p_, p_, p_, s_, p_, p_,'\0',
    p_, p_, p_, p_, p_, p_, p_, p_, p_, s_, p_, p_, p_, p_, p_, p_, p_, s_, p_, p_,'\0',
    p_, p_, p_, p_, p_, p_, p_, p_, p_, s_, p_, p_, p_, p_, p_, p_, p_, s_, p_, p_,'\0',
    p_, p_, s_, p_, p_, p_, p_, p_, p_, s_, p_, p_, p_, p_, p_, p_, p_, s_, p_, p_,'\0',
    p_, p_, s_, p_, p_, p_, p_, p_, p_, s_, p_, p_, p_, p_, p_, p_, p_, s_, p_, p_,'\0',
    p_, p_, s_, p_, p_, p_, p_, p_, p_, s_, p_, p_, p_, p_, p_, p_, p_, s_, p_, p_,'\0',
    p_, p_, s_, p_, p_, p_, p_, p_, p_, s_, p_, p_, p_, p_, p_, p_, p_, s_, p_, p_,'\0',
    p_, p_, s_, p_, p_, p_, p_, p_, p_, s_, p_, p_, p_, p_, p_, p_, p_, s_, p_, p_,'\0',
    p_, p_, s_, p_, p_, p_, p_, p_, p_, s_, s_, s_, s_, s_, s_, s_, s_, s_, p_, p_,'\0',
    p_, p_, s_, p_, p_, p_, p_, p_, p_, s_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_,'\0',
    p_, p_, s_, s_, s_, s_, s_, s_, s_, s_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_,'\0',
    p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_,'\0',
    p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_,'\0',
    p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_, p_,'\0'
};

//
// 起始点与终点
//
POINT start = { 4, 8 };
POINT endp = { 13, 6 };

//
// 开放列表与封闭列表
//
list< TILE * > open_list;
list< TILE * > close_list;

//
// 寻路成功后用于保存路径
//
TILE * path = NULL;

//
// 添加新节点到 open list
//
void AddNodeToOpenList( TILE * father, int x, int y )
{
    TILE * node = new TILE( x, y );
    
    node->father = father;
    
    if( father )
    {
        node->g = father->g + 1;
        node->h = abs( x - endp.x ) + abs( y - endp.y );
        node->f = node->g + node->h;
    }
    else
    {
        node->g = 0;
        node->h = 0;
        node->f = 0;
    }
    
    open_list.push_back( node );
}

//
// 判断节点是否已经存在 open list 里面
//
bool NodeInOpenList( int x, int y )
{
    list< TILE* >::iterator iter = open_list.begin( );
    while( iter != open_list.end( ) )
    {
        if( x == ( * iter )->x && y == ( * iter )->y )
            return true;
        iter ++;
    }
    
    return false;
}

//
// 添加特定节点的邻近节点到 open list
//
void AddAdjNodeToOpenList( TILE * father )
{
    int x = father->x;
    int y = father->y;
    
    // 生成四个邻近节点的 tile 索引
    
    POINT adj[ 4 ] =
    {
        { x, y - 1 },
        { x, y + 1 },
        { x - 1, y },
        { x + 1, y }
    };
    
    for( int i = 0; i < 4; i ++ )
    {
        // 节点必须在地图范围内
        
        if( adj[ i ].x >= 0 && adj[ i ].x < MAP_WIDTH &&
           adj[ i ].y >= 0 && adj[ i ].y < MAP_HEIGHT )
        {
            // 节点必须可以通行
            
            if( g_idle_point == map[ adj[ i ].y ][ adj[ i ].x ] )
            {
                // 节点不存在 open list 里面
                
                if( ! NodeInOpenList( adj[ i ].x, adj[ i ].y ) )
                {
                    // 满足上面三个条件的话，就把它添加到 open list 里面
                    
                    AddNodeToOpenList( father, adj[ i ].x, adj[ i ].y );
                }
            }
        }
    }
}

//
// 从 open list 中寻找最小 F 评价的节点
//
TILE * FindBestFNode( void )
{
    int min_f = 99999999;
    TILE * min_node = NULL;
    list< TILE* >::iterator iter = open_list.begin( );
    
    while( iter != open_list.end( ) )
    {
        if( ( * iter )->f < min_f )
        {
            min_f = ( * iter )->f;
            min_node = * iter;
        }
        
        iter ++;
    }
    
    return min_node;
}

//
// 从 open list 里面寻找 G 评价优于 node 的邻近节点
//
TILE * FindAdjBestGNode( TILE * node )
{
    int min_g = node->g;
    TILE * min_node = NULL;
    list< TILE* >::iterator iter = open_list.begin( );
    
    int x = node->x;
    int y = node->y;
    
    POINT adj[ 4 ] =
    {
        { x, y - 1 },
        { x, y + 1 },
        { x - 1, y },
        { x + 1, y }
    };
    
    for( int i = 0; i < 4; i ++ )
    {
        while( iter != open_list.end( ) )
        {
            TILE * temp = * iter;
            
            if( temp->x == adj[ i ].x && temp->y == adj[ i ].y )
            {
                if( temp->g <= min_g )
                {
                    min_g = temp->g;
                    min_node = temp;
                    break;
                }
            }
            
            iter ++;
        }
    }
    
    return min_node;
}

//
// 开始寻路, 仅进行四方向寻路
//
TILE * FindPath( int sx, int sy, int ex, int ey )
{
    TILE * start_node = NULL;
    TILE * best_f_node = NULL;
    TILE * best_g_node = NULL;
    
    // 清空 open list 和 close list
    
    open_list.clear( );
    close_list.clear( );
    
    // 添加起始节点到 open list
    
    AddNodeToOpenList( NULL, start.x, start.y );
    
    // 得到起始节点
    
    start_node = * open_list.begin( );
    
    // 添加起始节点的邻近节点到 open list
    
    AddAdjNodeToOpenList( start_node );
    
    // 开始寻路
    
    while( 0 != open_list.size( ) )
    {
        // 找到拥有最优 F 评价的节点
        
        best_f_node = FindBestFNode( );
        
        // 如果是目标节点, 则完成寻路
        // 此时 best_f_node 代表目标节点
        
        if(best_f_node
           && best_f_node->x == endp.x
           && best_f_node->y == endp.y )
            return best_f_node;
        
        // 将最优 F 评价的节点放入到 close list 里面
        // 并且将其从 open list 里面移除
        
        close_list.push_back( best_f_node );
        open_list.remove( best_f_node );
        
        // 我们看看 best_f_node 的邻近节点中
        // 是否存在比 best_f_node 拥有更优 G 评价的节点
        
        best_g_node = FindAdjBestGNode( best_f_node );
        if( best_g_node )
            best_f_node = best_g_node;
        
        // 将 best_f_node 节点的邻近节点添加到 open list 里面
        AddAdjNodeToOpenList( best_f_node );
    }
    return NULL;
}